/**
 * @file main.c
 * @author sylvain.gaeremynck@parrot.com
 * @date 2009/07/01
 */
#include <ardrone_testing_tool.h>

//ARDroneLib
#include <utils/ardrone_time.h>
#include <ardrone_tool/Navdata/ardrone_navdata_client.h>
#include <ardrone_tool/Control/ardrone_control.h>
#include <ardrone_tool/UI/ardrone_input.h>
#include <ardrone_tool/ardrone_tool_configuration.h>

//Common
#include <config.h>
#include <ardrone_api.h>

//VP_SDK
#include <ATcodec/ATcodec_api.h>
#include <VP_Os/vp_os_print.h>
#include <VP_Api/vp_api_thread_helper.h>
#include <VP_Os/vp_os_signal.h>

//Local project
//#include <Video/video_stage.h>
#include <c_interface.h>
#include <detect.h>
#include <ardrone_tool/Video/video_stage.h>
#include <ardrone_tool/Video/video_recorder_pipeline.h>

static int32_t exit_ihm_program = 1;

codec_type_t codec = H264_360P_CODEC;
ZAP_VIDEO_CHANNEL videoChannel = ZAP_CHANNEL_HORI;

/* Implementing Custom methods for the main function of an ARDrone application */
int main(int argc, char** argv)
{
    //printf("%d", ardp_testfunc());
    //ardp_initialize_image("im1.png");
//     ardp_initialize_detection(5, 0, 0);
//     int x, y, w, h;
//     char* data=ardp_get_testimage();
//     ardp_detect(data, &x, &y, &w, &h);
//     printf("%d%d%d%d", x, y, w, h);
    return ardrone_tool_main(argc, argv);
}

/* The delegate object calls this method during initialization of an ARDrone application */
C_RESULT ardrone_tool_init_custom(void)
{
    /* Registering for a new device of game controller */
    // ardrone_tool_input_add( &gamepad );


    ardrone_application_default_config.navdata_demo = TRUE;
    ardrone_application_default_config.navdata_options = (NAVDATA_OPTION_MASK(NAVDATA_DEMO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_VISION_DETECT_TAG) | NAVDATA_OPTION_MASK(NAVDATA_GAMES_TAG) | NAVDATA_OPTION_MASK(NAVDATA_MAGNETO_TAG) | NAVDATA_OPTION_MASK(NAVDATA_HDVIDEO_STREAM_TAG) | NAVDATA_OPTION_MASK(NAVDATA_WIFI_TAG));
    ardrone_application_default_config.video_codec = codec;
    

    ardrone_application_default_config.video_channel = videoChannel;
    ardrone_application_default_config.bitrate_ctrl_mode = 1;

    
    specific_parameters_t *params = (specific_parameters_t *)vp_os_calloc (1, sizeof (specific_parameters_t));
    specific_stages_t *example_pre_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
    specific_stages_t *example_post_stages = (specific_stages_t *)vp_os_calloc (1, sizeof (specific_stages_t));
    vp_api_picture_t *in_picture = (vp_api_picture_t *)vp_os_calloc (1, sizeof (vp_api_picture_t));
    vp_api_picture_t *out_picture = (vp_api_picture_t *)vp_os_calloc (1, sizeof (vp_api_picture_t));
    
    
//     in_picture->width = 640; // Drone 1 only : Must be greater than the drone 1 picture size (320)
//     in_picture->height = 360; // Drone 1 only : Must be greater that the drone 1 picture size (240)
// 
//     out_picture->framerate = 20; // Drone 1 only, must be equal to drone target FPS
    out_picture->format = PIX_FMT_RGB24; // MANDATORY ! Only RGB24, RGB565 are supported
//     out_picture->width = in_picture->width;
//     out_picture->height = in_picture->height;
    
    //not sure if this necessary
    uint32_t bpp;
        bpp = 3;
        out_picture->y_buf = vp_os_malloc ( out_picture->width * out_picture->height * bpp );
        out_picture->cr_buf = NULL;
        out_picture->cb_buf = NULL;
        out_picture->y_line_size = out_picture->width * bpp; //is this 0?
        out_picture->cb_line_size = 0;
        out_picture->cr_line_size = 0;
    
    example_post_stages->stages_list = (vp_api_io_stage_t *)vp_os_calloc (1, sizeof (vp_api_io_stage_t));
    
    
    int stages_index = 0;

    //temporary headers
    vp_api_stage_funcs_t detect_funcs = {
    NULL,
    (vp_api_stage_open_t) detect_stage_open,
    (vp_api_stage_transform_t) detect_stage_transform,
    (vp_api_stage_close_t) detect_stage_close
    };
    
    
    //vp_os_memset (&dispCfg, 0, sizeof (display_stage_cfg_t));
    //dispCfg.bpp = bpp;
    //dispCfg.decoder_info = in_picture;
    
    example_pre_stages->length=0;

    example_post_stages->stages_list[stages_index].name = "Detecting display"; // Debug info
    example_post_stages->stages_list[stages_index].type = VP_API_OUTPUT_SDL; // Debug info
    example_post_stages->stages_list[stages_index].cfg  = NULL;
    example_post_stages->stages_list[stages_index++].funcs  = detect_funcs;

    example_post_stages->length = stages_index;
    
    
    params->in_pic = in_picture;
    params->out_pic = out_picture;
    params->pre_processing_stages_list  = example_pre_stages;
    params->post_processing_stages_list = example_post_stages;
    params->needSetPriority = 0;
    params->priority = 0;
    /* Start all threads of your application */
     START_THREAD(video_stage, params);
     video_stage_init();
             START_THREAD (video_recorder, NULL);
             video_recorder_init ();
     video_stage_resume_thread ();

    return C_OK;
}

/* The delegate object calls this method when the event loop exit */
C_RESULT ardrone_tool_shutdown_custom(void)
{
    /* Relinquish all threads of your application */
    video_stage_resume_thread(); //Resume thread to kill it !
    JOIN_THREAD( video_stage );

        video_recorder_resume_thread ();
        JOIN_THREAD (video_recorder);
    /* Unregistering for the current device */
    // ardrone_tool_input_remove( &gamepad );

    return C_OK;
}

/* The event loop calls this method for the exit condition */
bool_t ardrone_tool_exit()
{
    return exit_ihm_program == 0;
}

C_RESULT signal_exit()
{
    exit_ihm_program = 0;

    return C_OK;
}

/* Implementing thread table in which you add routines of your application and those provided by the SDK */
BEGIN_THREAD_TABLE
THREAD_TABLE_ENTRY( ardrone_control, 20 )
THREAD_TABLE_ENTRY( navdata_update, 20 )
THREAD_TABLE_ENTRY( video_stage, 20 )
THREAD_TABLE_ENTRY(video_recorder, 20)
THREAD_TABLE_ENTRY( detect_stage, 20)
END_THREAD_TABLE

